#ifndef OPERATOR_H
#define OPERATOR_H


#include <NL/nl.h>

#include <fstream>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <set>
#include <cmath>
#include <cassert>
#include <string>
#include <iostream>
#include <deque>
#include <algorithm>
#include <sstream>

#include <algorithm>
#include <set>
#include <GL/glut.h>
#include <GL/GLU.h>

#include "utility.h"
#include "mesh.h"

//This class do not contain content of a mesh. Just reindexing vertex and selecting 
//some face in the original mesh.

class PseudoMesh 
{
public:
	PseudoMesh (Mesh * mesh): mesh (mesh)
	{

	}
public:
	inline unsigned pseudo2mesh_idx (unsigned pseudo_idx) const
	{
		return idx_pseudo2mesh[pseudo_idx];
	}
	inline unsigned mesh2pseudo_idx (unsigned mesh_idx) const
	{
		return idx_mesh2pseudo[mesh_idx];
	}

public: 
	inline Mesh * getMesh () {return this->mesh;}
	inline void  Face2MeshVertexIdx (Face * face, unsigned idx[3]) 
	{
		mesh->Face2VertexIdx(face, idx );
		idx[0] = pseudo2mesh_idx(idx[0]);
		idx[1] = pseudo2mesh_idx(idx[1]);
		idx[2] = pseudo2mesh_idx(idx[2]);
	}
	inline void  Face2VertexIdx (Face * face, unsigned idx[3]) 
	{
		mesh->Face2VertexIdx(face, idx );
	}
	inline void  Face2TexCoord (Face * face, Point2 v[3]) 
	{
		mesh->Face2TexCoord(face, v );
	}
	inline void  Face2VertexCoord (Face * face, Point3 v[3])
	{
		mesh->Face2VertexCoord (face, v);
	}
	//The face pointers point to the original face
	std::vector<Face * > v_faces; 
	std::vector<unsigned> idx_mesh2pseudo ;
	std::vector<unsigned> idx_pseudo2mesh ;
private :
	Mesh * mesh; 



};
//All the operators are applied on a mesh object
class Operator 
{
public:
	Operator(Mesh * mesh)  {}
	void getIdxResult2Operand (std::vector<unsigned> & idx)
	{
		//idx = this->idx_result2operand;
	}
	virtual ~ Operator () { }
	virtual operator PseudoMesh * () 
	{
		return pMesh ;
	} 
protected:

protected:
	//Construct New mesh from faces iter
	void constructMesh (std::vector<Face * > & faces)
	{
		constructVertex(faces);
	}
	void constructVertex (std::vector<Face * > & faces)
	{
		std::set < unsigned > resultVertexSet;
		for (std::vector<Face * >::iterator iter = faces.begin(); 
			iter != faces.end(); ++iter)
		{
			(*iter)->reached = false;
			(*iter)->covered = false;
			unsigned idx[3] ;
			pMesh->Face2VertexIdx(*iter, idx );
			resultVertexSet.insert(idx[0]); 
			resultVertexSet.insert(idx[1]);
			resultVertexSet.insert(idx[2]);
		}
		pMesh->idx_mesh2pseudo.resize(pMesh->getMesh()->vertices.size());
		pMesh->idx_pseudo2mesh.resize(resultVertexSet.size());

		unsigned count = 0;
		for (std::set<unsigned>::const_iterator iter = resultVertexSet.cbegin();
			iter != resultVertexSet.cend(); ++iter)
		{
			pMesh->idx_pseudo2mesh[count] = *iter;
			pMesh->idx_mesh2pseudo[*iter] = count;
			count ++ ;
		}
	}

protected:
	PseudoMesh * pMesh;
	
} ;
class OPCulling : public Operator 
{
public:
	OPCulling (Mesh * mesh, double mvmatrix[16],  double projmatrix[16],  int viewport[4]): 
	  Operator(mesh), mvmatrix(mvmatrix), projmatrix(projmatrix), viewport(viewport)
	  {pMesh = new PseudoMesh(mesh); }
	
	virtual operator PseudoMesh * () 
	{
		Mesh * mesh = pMesh->getMesh();
		//std::vector<Face * > visibalFace;
		for (unsigned i = 0; i < mesh->v_faces.size() ; ++i)
		{
			
			Point p[3];
			p[0] = mappingObj2Win(mesh->v_faces[i]->he->from_v->point, mvmatrix, projmatrix, viewport);
			p[1] = mappingObj2Win(mesh->v_faces[i]->he->next->from_v->point, mvmatrix, projmatrix, viewport);
			p[2] = mappingObj2Win(mesh->v_faces[i]->he->next->next->from_v->point, mvmatrix, projmatrix, viewport);

			Vector v1 = p[1]-p[0];
			Vector v2 = p[2]-p[0];
			Vector v3 = v1^v2 ;
			if (v3.z > 0)
			{
				//draw_tex_faces (i);
				pMesh->v_faces.push_back(mesh->v_faces[i]);
			}
		}
		constructVertex(pMesh->v_faces);
		return pMesh;
	}
	~ OPCulling () {}
protected:
	static Vector3 mappingObj2Win (Vector3 point, 	double mvmatrix[16],  double projmatrix[16],  int viewport[4])
	{

		double winx, winy, winz; 

		gluProject (point.x, point.y, point.z, mvmatrix, projmatrix, viewport, & winx, & winy, & winz);
		Vector3 result;

		result.x = float(winx);
		result.y = float(winy);
		result.z = 0.0f;
		return result;
	}
protected:
	double * mvmatrix ;
	double * projmatrix;
	int * viewport;
};

class OPBFSCulling : public Operator 
{
public:
	OPBFSCulling (Mesh * mesh, Brush * brush ): 
	  Operator(mesh), brush(brush), bfs_start_face(NULL)
	  {pMesh = new PseudoMesh(mesh); }
	
	virtual operator PseudoMesh * () 
	{
		//1. Find all the tri convered
		//2. Start from the center of the brush 
		//##choose the triangle which is nearest to the center of the triangles
		//3. DO BFS until convered all the triangles covered by the brush
		Mesh * mesh = pMesh->getMesh();
		float distance = 1.0e5 ;
		bfs_start_face = NULL; 
		int count = 0; // count how many faces is covered by brush;
		for (unsigned i = 0; i < mesh->v_faces.size() ; ++i)
		{
			
			Point p[3];
			mesh->v_faces[i]->Face2VertexCoord(p);
			if ( (brush->isInBrush(p[0]) || brush->isInBrush(p[1]) || brush->isInBrush(p[2])) && brush->isVisible(p))
			{
				count ++ ;
				mesh->v_faces[i]->covered = true; 
				//

				//Selected the visible face nearest to the center of the brush
				//we only test one vertex for each triangle that's enough
				Vector3 winPointCoord = brush->mappingObj2Win((p[0] + p[1] + p[2] )/3.0);
				Vector2 winCoord (winPointCoord.x, winPointCoord.y);
				Vector2 brushCenterCoord = Vector2(brush->get_winx(), brush->get_winy());
				Vector2 segment = winCoord - brushCenterCoord ;
				float length = segment.length();
				if (length < distance)
				{
					bfs_start_face = mesh->v_faces[i]; 
					distance = length ; 
				}
			}
		}
		//Do bfs on bfs_start_vertex unitl reach all the face covered
		//Point p3 = brush->mappingObj2Win(bfs_start_vertex->point) ;
		std::deque <Face * > faceDeque ;
		faceDeque.push_back(bfs_start_face);
		
		/*
		int_RGB c = rand_color();
		for (int i = 0; i < 6; ++i)
		{
			bfs_start_face->samples[i] = c;
		}
		*/
		while (count > 0 && faceDeque.size() > 0)
		{
			Face * front = faceDeque.front();
			pMesh->v_faces.push_back(front);
			front->reached = true; 
			faceDeque.pop_front(); 
			if (true == front->covered)
			{
				-- count;
			}
			//Traverse all the neighbor face of front which is not reached
			Vertex * v = front->he->from_v ;
			
			HalfEdge * HVIter = v->he ;
			HalfEdge * HVEnd = HVIter ;
			do 
			{
				if (false == HVIter->face->reached)
				{
					faceDeque.push_back(HVIter->face); 
					HVIter->face->reached = true;
				}
				HVIter = HVIter->flip->next ;
			}while (HVEnd != HVIter) ;

			v = front->he->next->from_v ;
			HVIter = v->he ;
			HVEnd = HVIter ;
			do 
			{
				if (false == HVIter->face->reached)
				{
					faceDeque.push_back(HVIter->face); 
					HVIter->face->reached = true;
				}
				HVIter = HVIter->flip->next ;
			}while (HVEnd != HVIter) ;

			v = front->he->next->next->from_v ;
			HVIter = v->he ;
			HVEnd = HVIter ;
			do 
			{
				if (false == HVIter->face->reached)
				{
					faceDeque.push_back(HVIter->face); 
					HVIter->face->reached = true;
				}
				HVIter = HVIter->flip->next ;
			}while (HVEnd != HVIter) ;
		}
		constructVertex(pMesh->v_faces);
		return pMesh;
	}
	~ OPBFSCulling () {}

	Face * getCenterTriangle () const
	{
		return bfs_start_face ;
	}
protected:
	Face * bfs_start_face ;

protected:
	void findBrushFront ()
	{
		
	}

	static Vector3 mappingObj2Win (Vector3 & point, double mvmatrix[16],  double projmatrix[16],  int viewport[4])
	{

		double winx, winy, winz; 

		gluProject (point.x, point.y, point.z, mvmatrix, projmatrix, viewport, & winx, & winy, & winz);
		Vector3 result;

		result.x = float(winx);
		result.y = float(winy);
		result.z = 0.0f;
		return result;
	}

protected:
	double * mvmatrix ;
	double * projmatrix;
	int * viewport;
	Brush * brush ;
};

class LSCM {
public:

    LSCM(PseudoMesh & m) : mesh_(&m) ,vxmin(NULL), vxmax(NULL)
	{
		verticesSize = mesh_->idx_pseudo2mesh.size();
    }
	~ LSCM() 
	{
		vxmin->locked = false;
		vxmax->locked = false;
	}
    void apply() {
		//int nb_vertices = mesh_->vertices.size() ;
		int nb_vertices = verticesSize ;
        project() ;
        nlNewContext() ;

		nlSolverParameteri(NL_SOLVER, NL_CG) ;
        nlSolverParameteri(NL_PRECONDITIONER, NL_PRECOND_JACOBI) ;
        nlSolverParameteri(NL_NB_VARIABLES, 2*nb_vertices) ;
        nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE) ;
        nlSolverParameteri(NL_MAX_ITERATIONS, 5*nb_vertices) ;
        nlSolverParameterd(NL_THRESHOLD, 1e-10) ;
        nlBegin(NL_SYSTEM) ;
        mesh_to_solver() ;
        nlBegin(NL_MATRIX) ;
        setup_lscm() ;
        nlEnd(NL_MATRIX) ;
        nlEnd(NL_SYSTEM) ;
        std::cout << "Solving ..." << std::endl ;
        nlSolve() ;
        solver_to_mesh() ;
        double time ;
        NLint iterations;
        nlGetDoublev(NL_ELAPSED_TIME, &time) ;
        nlGetIntergerv(NL_USED_ITERATIONS, &iterations); 
        std::cout << "Solver time: " << time << std::endl ;
        std::cout << "Used iterations: " << iterations << std::endl ;
        nlDeleteContext(nlGetCurrent()) ;
    }

protected:

    void setup_lscm() {
		std::cout<<"FACE SIZE "<<mesh_->v_faces.size()<<std::endl;
		for(unsigned int i=0; i<mesh_->v_faces.size() ; i++) {
            setup_lscm(mesh_->v_faces[i]) ;
        }
    }

    // Note: no-need to triangulate the facet,
    // we can do that "virtually", by creating triangles
    // radiating around vertex 0 of the facet.
    // (however, this may be invalid for concave facets)
    void setup_lscm(const Face  *  face) {  //FIXME HERE
		
		HalfEdge * iter = face->he;

		setup_conformal_map_relations( * mesh_->getMesh()->vertices[iter->from_v->idx] ,
			* mesh_->getMesh()->vertices[iter->next->from_v->idx], 
			* mesh_->getMesh()->vertices[iter->next->next->from_v->idx]) ;

    }

    // Computes the coordinates of the vertices of a triangle
    // in a local 2D orthonormal basis of the triangle's plane.
    static void project_triangle(
        const Vector3& p0, 
        const Vector3& p1, 
        const Vector3& p2,
        Vector2& z0,
        Vector2& z1,
        Vector2& z2
    ) {
        Vector3 X = p1 - p0 ;
        X.normalize() ;
        Vector3 Z = X ^ (p2 - p0) ;
        Z.normalize() ;
        Vector3 Y = Z ^ X ;
        const Vector3& O = p0 ;
        
        float x0 = 0 ;
        float y0 = 0 ;
        float x1 = (p1 - O).length() ;
        float y1 = 0 ;
        float x2 = (p2 - O) * X ;
        float y2 = (p2 - O) * Y ;        
            
        z0 = Vector2(x0,y0) ;
        z1 = Vector2(x1,y1) ;
        z2 = Vector2(x2,y2) ;        
    }

    // LSCM equation, geometric form :
    // (Z1 - Z0)(U2 - U0) = (Z2 - Z0)(U1 - U0)
    // Where Uk = uk + i.vk is the complex number 
    //                       corresponding to (u,v) coords
    //       Zk = xk + i.yk is the complex number 
    //                       corresponding to local (x,y) coords
    // cool: no divide with this expression,
    //  makes it more numerically stable in
    //  the presence of degenerate triangles.
    
    void setup_conformal_map_relations(
         Vertex& v0,  Vertex& v1,  Vertex& v2
    ) {
        
		int id0 = mesh_->mesh2pseudo_idx(v0.idx) ;
        int id1 = mesh_->mesh2pseudo_idx(v1.idx) ;
        int id2 = mesh_->mesh2pseudo_idx(v2.idx) ;
            
        const Vector3& p0 = v0.point ; 
        const Vector3& p1 = v1.point ;
        const Vector3& p2 = v2.point ;
            
        Vector2 z0,z1,z2 ;
        project_triangle(p0,p1,p2,z0,z1,z2) ;
        Vector2 z01 = z1 - z0 ;
        Vector2 z02 = z2 - z0 ;
        double a = z01.x ;
        double b = z01.y ;
        double c = z02.x ;
        double d = z02.y ;
        assert(b == 0.0) ;

        // Note  : 2*id + 0 --> u
        //         2*id + 1 --> v
        int u0_id = 2*id0     ;
        int v0_id = 2*id0 + 1 ;
        int u1_id = 2*id1     ;
        int v1_id = 2*id1 + 1 ;
        int u2_id = 2*id2     ;
        int v2_id = 2*id2 + 1 ;
        
        // Note : b = 0

        // Real part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -a+c)  ;
        nlCoefficient(v0_id,  b-d)  ;
        nlCoefficient(u1_id,   -c)  ;
        nlCoefficient(v1_id,    d)  ;
        nlCoefficient(u2_id,    a) ;
        nlEnd(NL_ROW) ;

        // Imaginary part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -b+d) ;
        nlCoefficient(v0_id, -a+c) ;
        nlCoefficient(u1_id,   -d) ;
        nlCoefficient(v1_id,   -c) ;
        nlCoefficient(v2_id,    a) ;
        nlEnd(NL_ROW) ;
    }

    /**
     * copies u,v coordinates from OpenNL solver to the mesh.
     */
    void solver_to_mesh() {
		for(unsigned int i=0; i< this->verticesSize; i++) {
			Vertex * it = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)]; 
			float u = nlGetVariable(2 * i    ) ;
            float v = nlGetVariable(2 * i + 1) ;
            it->tex_coord = Vector2(u,v) ;
        }
    }
        
    /**
     * copies u,v coordinates from the mesh to OpenNL solver.
     */
    void mesh_to_solver() {
		for(unsigned int i=0; i<this->verticesSize; i++) {
            Vertex * it = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            float u = it->tex_coord.x ;
            float v = it->tex_coord.y ;
			//std::cout<<u<<' '<<v<<std::endl;
            nlSetVariable(2 * i   , u) ;
            nlSetVariable(2 * i + 1, v) ;
            if(it->locked) {
                nlLockVariable(2 * i    ) ;
                nlLockVariable(2 * i + 1) ;
            } 
        }
    }

    // Chooses an initial solution, and locks two vertices
    void project() {
        // Get bbox
        unsigned int i ;

        float xmin =  1e5 ;
        float ymin =  1e5 ;
        float zmin =  1e5 ;
        float xmax = -1e5 ;
        float ymax = -1e5 ;
        float zmax = -1e5 ;

        for(i=0; i< verticesSize; ++i) {
            const Vertex *  v = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            xmin = min(v->point.x, xmin) ;
            ymin = min(v->point.y, xmin) ;
            zmin = min(v->point.z, xmin) ;

            xmax = max(v->point.x, xmin) ;
            ymax = max(v->point.y, xmin) ;
            zmax = max(v->point.z, xmin) ;
        }

        float dx = xmax - xmin ;
        float dy = ymax - ymin ;
        float dz = zmax - zmin ;
        
		std::cout<<"DX "<<dx<<std::endl;
		std::cout<<"DY "<<dy<<std::endl;
		std::cout<<"DZ "<<dz<<std::endl;
        Vector3 V1,V2 ;

        // Find shortest bbox axis
        if(dx < dy && dx < dz) {
            if(dy > dz) {
                V1 = Vector3(0,1,0) ;
                V2 = Vector3(0,0,1) ;
            } else {
                V2 = Vector3(0,1,0) ;
                V1 = Vector3(0,0,1) ;
            }
        } else if(dy < dx && dy < dz) {
            if(dx > dz) {
                V1 = Vector3(1,0,0) ;
                V2 = Vector3(0,0,1) ;
            } else {
                V2 = Vector3(1,0,0) ;
                V1 = Vector3(0,0,1) ;
            }
        } else  {
            if(dx > dy) {
                V1 = Vector3(1,0,0) ;
                V2 = Vector3(0,1,0) ;
            } else {
                V2 = Vector3(1,0,0) ;
                V1 = Vector3(0,1,0) ;
            }
        }

        // Project onto shortest bbox axis,
        // and lock extrema vertices

        //Vertex* vxmin = NULL ;
        float  umin = 1e5 ;
        
        float  umax = -1e5 ;

        for(i=0; i<verticesSize; i++) {
            Vertex * V = mesh_->getMesh()->vertices[mesh_->pseudo2mesh_idx(i)] ;
            float u = V->point * V1 ;
            float v = V->point * V2 ;
            V->tex_coord = Vector2(u,v) ;
            if(u < umin) {
                vxmin = V ;
                umin = u ;
            } 
            if(u > umax) {
                vxmax = V ;
                umax = u ;
            } 
        }

        vxmin->locked = true ;
        vxmax->locked = true ;

		//This fucntion is correct
		//std::cout<<"VMIN "<<vxmin->idx<<std::endl;
		//std::cout<<"VMAX "<<vxmax->idx<<std::endl;
    }

    PseudoMesh * mesh_ ;
private:
	Vertex* vxmin ;
	Vertex* vxmax ;
	unsigned verticesSize;
} ;


class OPLSCM 
{
public:
	OPLSCM (PseudoMesh * pmesh)  {this->pMesh = pmesh; }
	virtual operator PseudoMesh * () 
	{
		if (pMesh->idx_pseudo2mesh.size() > 2)
		{
			LSCM lscm (* pMesh) ;
			lscm.apply();
			
		}
		return pMesh ;
	}
	~OPLSCM () 
	{
	}
private:
	PseudoMesh * pMesh ; 
};


static bool compute_affine (Face * face, Point2 texPos, Point2 & trans, float & scale)
{
	Vector3 coord[3]; 
	face->Face2VertexCoord(coord);
	Vector2 texcoord[3];
	face->Face2TexCoord(texcoord);
	trans = texPos ;
	scale = (coord[0] - coord[1]).length () / (texcoord[0] - texcoord[1]).length (); 

	return true; 
}
class OPAffineTransformation 
{
public :
	OPAffineTransformation (PseudoMesh * pmesh, Vector2 & trans, float scale): trans(trans), scale(scale)
	{ this->pMesh = pmesh;}
	virtual operator PseudoMesh * ()
	{
		std::set<Vertex * > vertexSet ;
		for (unsigned i = 0;i < pMesh->v_faces.size(); ++i)
		{
			unsigned idx[3];
			pMesh->v_faces[i]->Face2VertexIdx(idx);
			vertexSet.insert(pMesh->getMesh()->vertices[idx[0]]);
			vertexSet.insert(pMesh->getMesh()->vertices[idx[1]]);
			vertexSet.insert(pMesh->getMesh()->vertices[idx[2]]);
		}
		//Do translate -> scaling -> translate back ;
		for (std::set<Vertex * >::iterator iter = vertexSet.begin(); iter != vertexSet.end(); ++iter)
		{
			(* iter)->tex_coord = (* iter)->tex_coord - trans ;
			(* iter)->tex_coord = (* iter)->tex_coord * scale ;
			//(* iter)->tex_coord = (* iter)->tex_coord + trans; 
		}
		return pMesh; 
	}
private:
	PseudoMesh * pMesh ; 
	Vector2 trans ;
	float scale ;
} ;
#endif